package com.javapda.c328r;

import gnu.io.SerialPort;

import java.io.ByteArrayOutputStream;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import com.javapda.c328r.command.ComediaCommand;
import com.javapda.c328r.command.ComediaDataPackage;
import com.javapda.c328r.command.ComediaErrorNumber;
import com.javapda.c328r.command.ComediaPictureType;
import com.javapda.c328r.command.ComediaPreviewJpegResolution;
import com.javapda.c328r.command.ComediaSnapshotType;
import com.javapda.c328r.command.impl.AckComediaCommand;
import com.javapda.c328r.command.impl.AckComediaCommandId;
import com.javapda.c328r.command.impl.DataComediaCommand;
import com.javapda.c328r.command.impl.GenericComediaCommandId;
import com.javapda.c328r.command.impl.GetPictureComediaCommand;
import com.javapda.c328r.command.impl.InitialComediaCommand;
import com.javapda.c328r.command.impl.NakComediaCommand;
import com.javapda.c328r.command.impl.NakComediaCommandId;
import com.javapda.c328r.command.impl.PowerOffComediaCommand;
import com.javapda.c328r.command.impl.RebootComediaCommand;
import com.javapda.c328r.command.impl.RequestEndPackageDataTransferComediaCommand;
import com.javapda.c328r.command.impl.RequestPackageDataComediaCommand;
import com.javapda.c328r.command.impl.ResetComediaCommand;
import com.javapda.c328r.command.impl.ResetStateMachinesComediaCommand;
import com.javapda.c328r.command.impl.SetPackageSizeComediaCommand;
import com.javapda.c328r.command.impl.SnapshotComediaCommand;
import com.javapda.c328r.command.impl.SyncComediaCommand;
import com.javapda.util.ByteUtil;
import com.javapda.util.MiscUtil;

public class C328rCameraImpl extends C328rCamera {
	private static final int COMEDIA_SYNC_MAX_TRIES = 60;
	private static final boolean verbose = false;
	private SerialPort serialPort;
	private OutputStream os;
	private InputStream is;
	private static final int MAX_RETRIES = 10;
	private int retryCount;
	private boolean on;
	public C328rCameraImpl(SerialPort _serialPort) {
		super();
		this.serialPort=_serialPort;
		if ( getConfiguration() == null ) {
			// fill from disk
		}
	}
	public boolean powerSwitch(boolean switchOn) {
		if ( switchOn ) {
			boolean powerSwitchStatus = sync();
			this.on = powerSwitchStatus;
			try {
				System.out.println("initializeCamera");
				initializeCamera();
				MiscUtil.pause(50);
				System.out.println("setPackageSize");
				MiscUtil.pause(50);
				setPackageSize();
				System.out.println("DONE: initializeCamera and setPackageSize");
			} catch (IOException e) {
				e.printStackTrace();
			}
		} else {
			powerOff();
			this.on = false;
		}
		return this.on;
		
	}
	private void powerOff() {
		// send power off to camera
		PowerOffComediaCommand cmd = new PowerOffComediaCommand();
		try {
			sendCommandToCamera(serialPort, cmd);
		} catch (IOException e) {
			e.printStackTrace();
		}
		System.out.println("Powered off");
		
	}
	
	private boolean sync() {
		System.out.print("Syncing...");
		boolean result = false;
		for ( int i = 0 ; i < COMEDIA_SYNC_MAX_TRIES && result == false; i++) {
			if ( verbose ) {
				System.out.print("Attempt " + (i+1) + "...");
			}
			try {
				result = attemptSyncWithCamera();
			} catch (IOException e) {
				e.printStackTrace();
			}
			//	System.out.println("Result = " + result);
		}
		if (result) {
			System.out.println("done with sync");
			// "After synchronizatin, the camera needs a little time for AEC and AGC to be stable.
			// ...Users should wait for 1-2 seconds before capturing the first picture."
			System.out.println("WAITING 2 seconds for camera to stabilize!");
			MiscUtil.pause(2000);
		} else {
			System.err.println("Unable to Sync Camera");
		}
		return result;
		
	}
	private boolean attemptSyncWithCamera() throws IOException {
		boolean result = false;
		InputStream in = serialPort.getInputStream();
		OutputStream out = serialPort.getOutputStream();
		SyncComediaCommand cmd = new SyncComediaCommand();
//		System.out.println(cmd.getDataAsString());
//		System.out.println(cmd.getData().length);
		int[] data = cmd.getData();
		for (int i = 0; i < data.length; i++) {
//			System.out.println("Write: " + data[i]);
			out.write(data[i]);
		}
		MiscUtil.pause(1000);
		int num = 0;
		int[] nums = new int[6];
		num = in.read();
		if (num!=-1) {
			result=true;
			nums[0] = num;
			for (int i = 1; i < nums.length; i++) {
				num=in.read();
				nums[i] = num;
			}
//			for (int i = 0; i < nums.length; i++) {
//				System.out.println("nums["+i+"]="+nums[i]);
//			}
		} else {
			// nothing read move on
		}
		
		GenericComediaCommandId gccid = new GenericComediaCommandId(nums[0],nums[1]);
		if ( (new AckComediaCommandId()).equals(gccid) ) {
			System.out.println("MATCH ACK");
			AckComediaCommand ack = new AckComediaCommand(nums);
			System.out.println("ACK: " + ack);
			result = getSyncFromCamera();
		} else if ((new NakComediaCommandId()).equals(gccid)){
			System.out.println("OUCH, it's a NAK");
			NakComediaCommand nak = new NakComediaCommand(nums);
			System.out.println("NAK: " + nak);
			System.out.println("ERROR: " + ComediaErrorNumber.getDescription(nak.getErrorNumber()));
		} else {
			if ( verbose ) { 
				System.out.println("NO MATCH ACK");
			}
		}
		
		
		return result;
	}

	private boolean getSyncFromCamera() throws IOException {
		System.out.println("Get Sync From Camera");
		InputStream in = serialPort.getInputStream();
		OutputStream out = serialPort.getOutputStream();
		int[] nums = new int[6];
		int num = in.read();
		boolean result = false;
		if (num!=-1) {
//			result=true;
			nums[0] = num;
			for (int i = 1; i < nums.length; i++) {
				num=in.read();
				nums[i] = num;
			}
			for (int i = 0; i < nums.length; i++) {
				System.out.println(nums[i]);
			}
			// send ACK to camera, with this id
			if ( verbose ) {
				System.out.println("TIME TO SEND ACK TO CAMERA");
			}
			AckComediaCommand ack = new AckComediaCommand();
			ack.setCmdId(nums[1]);
			ComediaCommand cmd = ack;
			int[] data = cmd.getData();
			for (int i = 0; i < data.length; i++) {
				if ( verbose ) {
					System.out.println("Write: " + data[i]);
				}
				out.write(data[i]);
			}
			result = true;
		}		
		
		return result;
	}
	/**
	 * @return the on
	 */
	public boolean isOn() {
		return on;
	}
	public void setConfiguration(C328rCameraConfiguration configuration) {
		this.configuration=configuration;
		if ( !isOn() ) {
			powerSwitch(true);
		}
		try {
			initializeCamera();
		} catch (IOException e) {
			e.printStackTrace();
		}
	}

	private boolean isAck(ComediaCommand comediaCommand) {
		if ( comediaCommand==null ) {
			System.out.println("OUCH IT IS NULL CMD");
		}
		return comediaCommand.getData()[1] == 0x0E;
	}
	
	private ComediaCommand getAckNakFromCamera(SerialPort serialPort) throws IOException {
		InputStream in = serialPort.getInputStream();
		int[] nums = {5,5,5,5,5,5};
		MiscUtil.pause(50);
		nums=ByteUtil.readMultiByte(in, nums);
		GenericComediaCommandId gccid = new GenericComediaCommandId(nums[0],nums[1]);
		ComediaCommand response = null;
		if ( (new AckComediaCommandId()).equals(gccid) ) {
			response = new AckComediaCommand(nums);
		} else if ((new NakComediaCommandId()).equals(gccid)){
			response = new NakComediaCommand(nums);
		} else {
			System.out.println("NO MATCH ACK/NAK...re-try the command "+gccid);
			
		}
		return response;
	}
	
	private void sendCommandToCamera(SerialPort serialPort, ComediaCommand cmd) throws IOException {
//		System.out.println(">> "+cmd);
		OutputStream out = serialPort.getOutputStream();
		for (int i = 0; i < cmd.getData().length; i++) {
			out.write(cmd.getData()[i]);
		}
	}
	
	private void initializeCamera() throws IOException {
		OutputStream out = serialPort.getOutputStream();
		// send the init
		InitialComediaCommand cmd = new InitialComediaCommand();
		
		cmd.setPreviewColorType(configuration.getPreviewColorType());
		cmd.setPreviewResolution(configuration.getPreviewResolution());//ComediaPreviewJpegResolution.RES_640x480);
		// don't care about preview resolution
		cmd.setPreviewJpegResolution(configuration.getPreviewJpegResolution());//ComediaPreviewJpegResolution.RES_640x480);
		sendCommandToCamera(serialPort,cmd);
		MiscUtil.pause(150);
		
		// get an ack/nak
		ComediaCommand response=getAckNakFromCamera(serialPort);
		if ( isAck(response) ) {
			// All is well
			if ( verbose ) {
				System.out.println("HURAY, CAMERA INITIALIZED");
			}
		} else {
			// Problem 
			throw new RuntimeException("Problem NAK: " + ComediaErrorNumber.getDescription(((NakComediaCommand)response).getErrorNumber()));
		}
		
		
	}
	
	
	private void setPackageSize(SerialPort serialPort) throws IOException {
//		System.out.println("SET PACKAGE SIZE");
		// simplify the API here
		SetPackageSizeComediaCommand cmd = new SetPackageSizeComediaCommand();
		// 512 bytes, 0x0200, low=0x00 & high=0x02
		cmd.setPackageSizeLowByte(0x00);
		cmd.setPackageSizeHighByte(0x02);
		sendCommandToCamera(serialPort, cmd);
		// get an ack/nak
		ComediaCommand response=getAckNakFromCamera(serialPort);
		if ( response==null ) {
			setPackageSize(serialPort);
		} else if ( isAck(response) ) {
			// All is well
			if ( verbose ) {
				System.out.println("HURAY, CAMERA SET PACKAGE SIZE");
			}
		} else {
			// Problem 
			throw new RuntimeException("Problem NAK: " + response);
		}
		
	}

	private void snapshot(SerialPort serialPort) throws IOException {
		MiscUtil.pause(100);
		SnapshotComediaCommand cmd = new SnapshotComediaCommand();
		cmd.setSnapshotType(configuration.getSnapshotType());
		// no skip frame
		cmd.setSkipFrameHighByte(0x00);
		cmd.setSkipFrameLowByte(0x00);
		sendCommandToCamera(serialPort, cmd);
		// get an ack/nak
		ComediaCommand response=getAckNakFromCamera(serialPort);
		if ( isAck(response) ) {
			// All is well
			if ( verbose ) {
				System.out.println("HURAY, CAMERA SNAPSHOT");
			}
		} else {
			// Problem 
			throw new RuntimeException("Problem NAK: " + response);
		}
	}
	
	private void setPackageSize() {
		try {
			setPackageSize(serialPort);
		} catch (IOException e) {
			e.printStackTrace();
		}
	}
	
	private void snapshot() {
		try {
			snapshot(serialPort);
		} catch (IOException e) {
			e.printStackTrace();
		}
	}
	// Prereq's:  cam up and configured
	public byte[] getSnapshot() {
		// set package size command
		//setPackageSize();
		snapshot();
		byte[] data = getSnapshotPicture();
		return data;
	}
	public byte[] getJpegPreview() {
//		setPackageSize();
		byte[] data = getJpegPreviewPicture();
		return data;
	}
	private byte[] getJpegPreviewPicture() {
		MiscUtil.pause(100);
		// actually pull from the camera's buffer
		GetPictureComediaCommand cmd = new GetPictureComediaCommand(ComediaPictureType.JPEG_PREVIEW);
		try {
			sendCommandToCamera(serialPort, cmd);
		} catch (IOException e) {
			e.printStackTrace();
		}
		// pause based on resolution of requested image...
		MiscUtil.pause(400);
//		System.out.println("GET ACK/NACK after preview request");
		// get an ack/nak
		ComediaCommand response=null;
		boolean got = false;
		try {
			response = getAckNakFromCamera(serialPort);
//			if ( response != null ) {
//				System.out.println("GOT GOOD ACK");
//			} else {
//				MiscUtil.pause(100);
//				System.out.println("Try again...");
//			}
		} catch (IOException e) {
			e.printStackTrace();
		}
		if ( response == null ) {
			//return getJpegPreviewPicture();
			return null;
		} else if ( isAck(response) ) {
			// All is well
			if ( verbose ) {
				System.out.println("HURAY, CAMERA GET PICTURE");
			}
		} else {
			// Problem 
			NakComediaCommand nak = (NakComediaCommand)response;
			String errorMessage = ComediaErrorNumber.getDescription(nak.getErrorNumber());
			throw new RuntimeException("Problem NAK: " + response+"\n"+errorMessage);
		}
		return receiveDataFromCamera(ComediaSnapshotType.COMPRESSED == configuration.getSnapshotType());
	}
	private byte[] getSnapshotPicture() {
		// actually pull from the camera's buffer
		GetPictureComediaCommand cmd = new GetPictureComediaCommand();
		cmd.setPictureType(ComediaPictureType.SNAPSHOT);
		try {
			sendCommandToCamera(serialPort, cmd);
		} catch (IOException e) {
			e.printStackTrace();
		}
		// get an ack/nak
		ComediaCommand response=null;
		try {
			response = getAckNakFromCamera(serialPort);
		} catch (IOException e) {
			e.printStackTrace();
		}
		if ( isAck(response) ) {
			// All is well
			if ( verbose ) {
				System.out.println("HURAY, CAMERA GET PICTURE");
			}
		} else {
			// Problem 
			NakComediaCommand nak = (NakComediaCommand)response;
			String errorMessage = ComediaErrorNumber.getDescription(nak.getErrorNumber());
			throw new RuntimeException("Problem NAK: " + response+"\n"+errorMessage);
		}
		return receiveDataFromCamera(ComediaSnapshotType.COMPRESSED == configuration.getSnapshotType());
		
	}
	private byte[] receiveDataFromCamera(boolean compressed) {
		byte[] data = null;
		try {
			if ( compressed ) {
				data = receiveCompressedData(serialPort);
			} else {
				data = receiveUncompressedData(serialPort);
			}
		} catch(IOException e) {
			e.printStackTrace();
		}
		return data;
	}
	private byte[] receiveUncompressedData(SerialPort serialPort2) {
		throw new RuntimeException("NOT YET IMPLEMENTED");
	}
	
	private long getReceiveDelay() {
		long delay = 1100;
		if ( configuration.getPreviewJpegResolution()==ComediaPreviewJpegResolution.RES_80x64 ) delay=140;
		if ( configuration.getPreviewJpegResolution()==ComediaPreviewJpegResolution.RES_160x128 ) delay=200;
		if ( configuration.getPreviewJpegResolution()==ComediaPreviewJpegResolution.RES_320x240 ) delay=200;
		if ( configuration.getPreviewJpegResolution()==ComediaPreviewJpegResolution.RES_640x480 ) delay=1000;
//		System.err.println("Unknown jpeg resolution: "+configuration.getPreviewJpegResolution());
//		System.out.println("RECEIVE DELAY="+delay+" ms");
		return delay;
	}
	private byte[] receiveCompressedData(SerialPort serialPort2) throws IOException {
		MiscUtil.pause(getReceiveDelay());
		InputStream in = serialPort.getInputStream();
		int[] nums = readComediaCommand(serialPort);
		DataComediaCommand cmd = new DataComediaCommand(nums);
		sendAckToCamera(serialPort);
		// retrieve bytes of Data 
		int expectedImageSize = cmd.getImageSize();
		int numPackages = (int) Math.ceil(cmd.getImageSize()/((double)configuration.getPackageSize()-6));
		if ( verbose ) {
			System.out.println("@@ IMAGE SIZE: " + expectedImageSize);
			System.out.println("No. Packages (CEIL)="+numPackages);
		}
		List comediaDataPackageList = new ArrayList();
		
		for (int i = 0; i < numPackages; i++) {
			MiscUtil.pause(100);
				int sum = 0;
				
				// read in package id
				int[] packageIdBytes = { ByteUtil.readOneByte(in), ByteUtil.readOneByte(in) };
				sum+=packageIdBytes[0];
				sum+=packageIdBytes[1];
				int packageId = ByteUtil.intArrayToInt(packageIdBytes);
				
				
				// read in data size
				int[] imageDataSizeBytes = { ByteUtil.readOneByte(in), ByteUtil.readOneByte(in) };
				sum+=imageDataSizeBytes[0];
				sum+=imageDataSizeBytes[1];
				int imageDataSize = ByteUtil.intArrayToInt(imageDataSizeBytes);
				
				// read data size worth of data
				byte[] imageData = new byte[imageDataSize];
				for (int j = 0; j < imageData.length; j++) {
					int datum = ByteUtil.readOneByte(in);
					sum+=datum;
					imageData[j] = (byte) datum;
				}
//				in.read(imageData);
				
				// read in verify error
				int[] verifyErrorBytes = { ByteUtil.readOneByte(in), ByteUtil.readOneByte(in) };
				int verifyError = ByteUtil.intArrayToInt(verifyErrorBytes);
				boolean match = (((byte)sum)==((byte)verifyError));
				if ( verbose ) {
					System.out.println("PACKAGE VERIFY: " + ((byte)sum) + " vs. " + ((byte)verifyError) );
					System.out.println("PACKAGE VERIFY Status: " + match);
				}
				if ( !match ) {
					System.err.println("PACKAGE MISMATCH: packageid="+packageId);
				}
				
			ComediaDataPackage cdp = new ComediaDataPackage();
			cdp.setPackageId(packageId);
			cdp.setImageData(imageData);
			cdp.setVerifyError(verifyError);
			if ( verbose ) {
				System.out.println("Package ID="+cdp.getPackageId());
			}
			comediaDataPackageList.add(cdp);
			RequestPackageDataComediaCommand reqPackageComand = new RequestPackageDataComediaCommand();
			int nextPackageId = i+1;
			reqPackageComand.setPackageId(nextPackageId);
			sendCommandToCamera(serialPort, reqPackageComand);
		}
		// walk through all items and see what actual data size is
		byte[] imageBytes = convertImageDataToByteArray(comediaDataPackageList);
		//writeBytesToFile(imageBytes);
		RequestEndPackageDataTransferComediaCommand endCommand = new RequestEndPackageDataTransferComediaCommand();
		sendCommandToCamera(serialPort, endCommand);
		return imageBytes;
	}
	private void sendAckToCamera(SerialPort serialPort) throws IOException {
		// send ACK to camera
		AckComediaCommand ack = new AckComediaCommand();
		sendCommandToCamera(serialPort, ack);
	}

	
	private byte[] convertImageDataToByteArray(List dt)
	throws IOException {
		ByteArrayOutputStream baos = new ByteArrayOutputStream();
		for (Iterator iterator = dt.iterator(); iterator.hasNext();) {
			ComediaDataPackage cdp = (ComediaDataPackage) iterator.next();
			baos.write(cdp.getImageData());
		}
		return baos.toByteArray();
	}
	
	private int[] readComediaCommand(SerialPort serialPort) throws IOException {
		InputStream in = serialPort.getInputStream();
		int[] nums = new int[6];
		nums = ByteUtil.readMultiByte(in, nums);
//		for (int i = 0; i < nums.length; i++) {
//			nums[i] = in.read();
//		}
		return nums;
	}
	public void reset(boolean reboot) {
		ResetComediaCommand cmd = (reboot)?new RebootComediaCommand() :new ResetStateMachinesComediaCommand();
		try {
			sendCommandToCamera(serialPort, cmd);
		} catch (IOException e) {
			e.printStackTrace();
		}
		System.out.println("Powered off");
		
	}
	
}
